#!/home/he/anaconda3/envs/hellobot/bin/python
#  -*-coding:utf8 -*-

from ast import Is
import pybullet as p
import time
import pybullet_data
import rospy
from hellobot_msgs.msg import msg_sim
import numpy as np

v1 = 0
v2 = 0
dv = 0.1
wheel_radias = 0.055 # 轮半径，单位：m
#target pos
tx = -3
ty = 2
tz = 0
IsNewTarget = False
#targetTorobot
rtx = 0
rty = 0
rtz = 0

def v_reset():
    global v1,v2
    v1 = 0
    v2 = 0

def v_update(p,robot):
    p.setJointMotorControl2(bodyIndex=robot,
                                jointIndex=0,
                                controlMode=p.VELOCITY_CONTROL,
                                targetVelocity=v1/wheel_radias) 
    p.setJointMotorControl2(bodyIndex=robot,
                                jointIndex=1,
                                controlMode=p.VELOCITY_CONTROL,
                                targetVelocity=v2/wheel_radias)
    p.stepSimulation()

def KeyboardContrl(keys,p,robot):
    global v1,v2
    if ord('r') in keys and p.KEY_WAS_TRIGGERED:
        p.resetSimulation()
    if ord('u') in keys and p.KEY_IS_DOWN:
        v1 = v1 + dv
        p.setJointMotorControl2(bodyIndex=robot,
                                jointIndex=0,
                                controlMode=p.VELOCITY_CONTROL,
                                targetVelocity=v1)        # Velocity Control, Search this in Online Doc
    if ord('j') in keys and p.KEY_IS_DOWN:
        v1 = v1 - dv
        p.setJointMotorControl2(bodyIndex=robot,
                                jointIndex=0,
                                controlMode=p.VELOCITY_CONTROL,
                                targetVelocity=v1)
    if ord('i') in keys and p.KEY_IS_DOWN:                  # Watchout!!!! The positive direction is reversed
        v2 = v2 + dv
        p.setJointMotorControl2(bodyIndex=robot,
                                jointIndex=1,
                                controlMode=p.VELOCITY_CONTROL,
                                targetVelocity=v2)
    if ord('k') in keys and p.KEY_IS_DOWN:
        v2 = v2 - dv
        p.setJointMotorControl2(bodyIndex=robot,
                                jointIndex=1,
                                controlMode=p.VELOCITY_CONTROL,
                                targetVelocity=v2)
    p.stepSimulation()
    #time.sleep(1./240.)
    
    # time.sleep(1./240.)
    # if i%100==0:
    #     print(p.getBaseVelocity(bodyUniqueId=robot))    # This line Get the velocity of the base body
    #     print(p.getJointState(bodyUniqueId=robot,
    #                         jointIndex=0))        # This gets the joint state
    #     print(p.getJointState(bodyUniqueId=robot,
    #                         jointIndex=1))
        # getLinkState will give you the link states, like link velocity, orientation
    #p.disconnect()
    return v2,v1

def MsgUpdate(p,robot):
    msg = msg_sim()
    msg.vl = v1
    msg.vr = v2
    robot_PosAndOri = p.getBasePositionAndOrientation(bodyUniqueId=robot)
    msg.rx = robot_PosAndOri[0][0]
    msg.ry = robot_PosAndOri[0][1]
    msg.rz = robot_PosAndOri[0][2]
    eular = p.getEulerFromQuaternion(robot_PosAndOri[1][:])# alpha beta gamma
    #print(eular)
    msg.ralpha = eular[0]
    msg.rbeta = eular[1]
    msg.rgamma = eular[2]
    msg.tx = rtx
    msg.ty = rty
    msg.tz = rtz
    msg.newstatus = IsNewTarget

    return msg

def targetSet(p,robot):
    # robot pos
    robot_PosAndOri = p.getBasePositionAndOrientation(bodyUniqueId=robot)
    rx = robot_PosAndOri[0][0]
    ry = robot_PosAndOri[0][1]
    rz = robot_PosAndOri[0][2]

    # duck
    shift = [0, -0.02, 0]
    meshScale = [0.1, 0.1, 0.1]
    visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
                                fileName="duck.obj",
                                rgbaColor=[1, 1, 1, 1],
                                specularColor=[0.4, .4, 0],
                                visualFramePosition=shift,
                                #visualFrameOrientation=[0.7071,0,0,0.7071],
                                meshScale=meshScale)
    collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
                                            fileName="duck_vhacd.obj",
                                            collisionFramePosition=shift,
                                            meshScale=meshScale)


    p.createMultiBody(baseMass=1,
                        baseInertialFramePosition=[0, 0, 0],
                        baseCollisionShapeIndex=collisionShapeId,
                        baseVisualShapeIndex=visualShapeId,
                        basePosition=[tx, ty, 0],                     ##修改这一行来改变鸭子的位置（但Z方向不会固定 而是会下落）修正坐标系xyz逆时针90度
                        useMaximalCoordinates=True)
    
    rospy.loginfo("Rx,Ry,Tx,Ty[%0.4f , %0.4f , %0.4f ,%0.4f]", 
                    rx, ry, tx, ty)

class Simulation():
    def __init__(self):
        self.pub = rospy.Publisher('SIM', msg_sim, queue_size=1)
        rospy.Subscriber('Motor_Speed', msg_sim, self.callbackSpeed)
        rate = rospy.Rate(60)

        physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
        p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
        p.setGravity(0,0,-10)
        p.setTimeStep(1. / 60.)
        planeId = p.loadURDF("plane.urdf")
        startPos = [0,0,0.055]
        startOrientation = p.getQuaternionFromEuler([0,0,0])     # Initial Orientation, play with it yourself
        #robot = p.loadURDF("./Simulation/urdf/Simulation.urdf",startPos, startOrientation)
        robot = p.loadURDF("/home/he/hellobot/src/hellobot_sim/src/PybulletSimulation/Simulation/urdf/Simulation.urdf",startPos, startOrientation)

        p.resetDebugVisualizerCamera(2.3,90,-60,[-1,2,0])        # Set default Camera Position, press ctrl+drag to change view
        #p.resetDebugVisualizerCamera(1,0,-55,[-5,3,0]) 
        # p.changeVisualShape(robot,0,rgbaColor=[0.1,0.1,0.1,0.1])      # This Line Change the color and Transparency of body

        #set the center of mass frame (loadURDF sets base link frame) startPos/Ornp.resetBasePositionAndOrientation(boxId, startPos, startOrientation)
        v_reset()
        time_begin = time.time()
        count = 0
        global IsNewTarget,tx,ty,rtx,rty
        while not rospy.is_shutdown():
            #for i in range (10000):
            #key j is vl forwatd(-v1), i is vr forward(+v2). key u,k are back.
            #key r will cause a bug?
            # keys = p.getKeyboardEvents()
            # KeyboardContrl(keys,p,robot)
		    
            v_update(p,robot)

            # 仿真时间设置

            # 发布消息
            msg = MsgUpdate(p,robot)
            self.pub.publish(msg)
            rospy.loginfo("Publish[%0.4f m/s, %0.4f m/s, %0.3f degree]", 
                    msg.vl, msg.vr, msg.rgamma/np.pi*180)

            time_used = time.time()-time_begin
            #print(time_used)
            if count == 0:
                if time_used > 3:
                    IsNewTarget = True
                    #tx = -3
                    #ty = 2
                    robot_PosAndOri = p.getBasePositionAndOrientation(bodyUniqueId=robot)
                    rx = robot_PosAndOri[0][0]
                    ry = robot_PosAndOri[0][1]
                    rz = robot_PosAndOri[0][2]
                    l = ((-3-rx)**2+(2-ry)**2)**0.5
                    dtx = tx - rx
                    dty = ty - ry
                    # rtx = -dtx * np.sin(msg.rgamma) + dty * np.cos(msg.rgamma)
                    # rty = -dtx * np.cos(msg.rgamma) - dty * np.sin(msg.rgamma)
                    rtx = dtx * np.cos(np.pi/2 + msg.rgamma) + dty * np.sin(np.pi/2 + msg.rgamma)
                    rty = -dtx * np.sin(np.pi/2 + msg.rgamma) + dty * np.cos(np.pi/2 + msg.rgamma)

                    # theta = np.arccos((ty-ry)/l)
                    # tx = l*np.cos(theta-msg.rgamma)
                    # ty = l*np.sin(theta-msg.rgamma)
                    msg = MsgUpdate(p,robot)
                    
                    self.pub.publish(msg)
                    rospy.loginfo("Publish a new Target!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!tx, ty: %0.2f, %0.2f; rtx, rty: %0.2f, %0.2f",tx,ty,rtx,rty) 
                #if time_used > 5.1:
                    targetSet(p,robot)
                    IsNewTarget = False
                    count = 1
            # if count == 1:
            #     if time_used > 10:
            #         IsNewTarget = True
            #         #tx = -2
            #         #ty = 4
            #         tx=-2
            #         ty=4
            #         robot_PosAndOri = p.getBasePositionAndOrientation(bodyUniqueId=robot)
            #         rx = robot_PosAndOri[0][0]
            #         ry = robot_PosAndOri[0][1]
            #         rz = robot_PosAndOri[0][2]
            #         l = ((-3-rx)**2+(2-ry)**2)**0.5
            #         dtx = tx - rx
            #         dty = ty - ry
            #         # rtx = -dtx * np.sin(msg.rgamma) + dty * np.cos(msg.rgamma)
            #         # rty = -dtx * np.cos(msg.rgamma) - dty * np.sin(msg.rgamma)
            #         rtx = dtx * np.cos(np.pi/2 + msg.rgamma) + dty * np.sin(np.pi/2 + msg.rgamma)
            #         rty = -dtx * np.sin(np.pi/2 + msg.rgamma) + dty * np.cos(np.pi/2 + msg.rgamma)

            #         # theta = np.arccos((ty-ry)/l)
            #         # tx = l*np.cos(theta-msg.rgamma)
            #         # ty = l*np.sin(theta-msg.rgamma)
            #         msg = MsgUpdate(p,robot)
                    
            #         self.pub.publish(msg)
            #         rospy.loginfo("Publish a new Target!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!tx, ty: %0.2f, %0.2f; rtx, rty: %0.2f, %0.2f",tx,ty,rtx,rty) 
            #     # if time_used > 10.1:
            #         # tx = 4.5
            #         # ty = 2.5
            #         targetSet(p,robot)
            #         IsNewTarget = False
            #         count = 2
            
            # 按照循环频率延时
            rate.sleep()
    
    def callbackSpeed(self, data):
        global v1, v2
        v1 = -data.vr
        v2 = data.vl

if __name__ == '__main__':
    rospy.init_node('Simulation')
    try:
        Simulation()
    except rospy.ROSInterruptException:
        pass
    rospy.spin()